/**
  ******************************************************************************
  * File Name          : gluon_state_publisher.cpp
  * Description       :  gluon_state_publisher program body
  ******************************************************************************
  *
  * Copyright (c) 2022 VSTC Co., Ltd. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of VSTC nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for VSTC.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY HOPEMOTION AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL HOPEMOTION OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include <gluon_state_publisher/gluon_state_publisher.h>

double joint_pos[JOINT_MAX] = {0,0,0,0,0,0};

/**
  * @brief  关节一接收回调函数
  * @param  消息结构体指针
  * @retval 无
  */
void j1_CB(const std_msgs::Float64::ConstPtr & msg)
{
  double joint = msg->data;
  joint_pos[0] = joint;
}

/**
  * @brief  关节二接收回调函数
  * @param  消息结构体指针
  * @retval 无
  */
void j2_CB(const std_msgs::Float64::ConstPtr & msg)
{
  double joint = msg->data;
  joint_pos[1] = joint;
}

/**
  * @brief  关节三接收回调函数
  * @param  消息结构体指针
  * @retval 无
  */
void j3_CB(const std_msgs::Float64::ConstPtr & msg)
{
  double joint = msg->data;
  joint_pos[2] = joint;
}

/**
  * @brief  关节四接收回调函数
  * @param  消息结构体指针
  * @retval 无
  */
void j4_CB(const std_msgs::Float64::ConstPtr & msg)
{
  double joint = msg->data;
  joint_pos[3] = joint;
}

/**
  * @brief  关节五接收回调函数
  * @param  消息结构体指针
  * @retval 无
  */
void j5_CB(const std_msgs::Float64::ConstPtr & msg)
{
  double joint = msg->data;
  joint_pos[4] = joint;
}

/**
  * @brief  关节六接收回调函数
  * @param  消息结构体指针
  * @retval 无
  */
void j6_CB(const std_msgs::Float64::ConstPtr & msg)
{
  double joint = msg->data;
  joint_pos[5] = joint;
}

/**
  * @brief  机械臂驱动主函数
  * @param  消息结构体指针
  * @retval 无
  */
int main(int argc, char** argv)
{
  ros::init(argc, argv, "gluon_state_publisher");
  ros::NodeHandle nh;
  ros::Rate loop_rate(50);

  //订阅角度话题
  ros::Subscriber sub_joint_1 = nh.subscribe("gluon/joint_1_position", 1, j1_CB);
  ros::Subscriber sub_joint_2 = nh.subscribe("gluon/joint_2_position", 1, j2_CB);
  ros::Subscriber sub_joint_3 = nh.subscribe("gluon/joint_3_position", 1, j3_CB);
  ros::Subscriber sub_joint_4 = nh.subscribe("gluon/joint_4_position", 1, j4_CB);
  ros::Subscriber sub_joint_5 = nh.subscribe("gluon/joint_5_position", 1, j5_CB);
  ros::Subscriber sub_joint_6 = nh.subscribe("gluon/joint_6_position", 1, j6_CB);

  //发布机械臂状态
  ros::Publisher pub_states = nh.advertise<sensor_msgs::JointState>("joint_states", 100);

  sensor_msgs::JointState joint_state;

  nh.getParam("/gluon_state_publisher/joint_names", joint_state.name);
  nh.getParam("/gluon_state_publisher/joint_position", joint_state.position);

  // for(int i=0; i< JOINT_MAX; i++)
  // {
  //    printf("%s\r\n", joint_state.name[i].c_str());
  // }

  while(ros::ok())
  {
    
    joint_state.header.stamp = ros::Time::now();
    for(int i=0; i<JOINT_MAX; i++)
    {
      joint_state.position[i] = joint_pos[i];  
    }
    pub_states.publish(joint_state);

    ros::spinOnce();
    loop_rate.sleep();
  }

  return 0;
}
